/******************************************************************************
 *  _____     _____
 *  \  __\    \  __\
 *   \ \_\\    \ \_\\
 *    \  __\SS  \  __\roject
 *     \_\       \_\
 *
 * Mateusz Maciąg, Rafał Urbański
 * Komputerowe Systemy Sterowania
 *****************************************************************************/

#ifndef GPCCONTROLLER_H
#define GPCCONTROLLER_H

#include "ControllerBase.h"
#include "WrmnkARXWrapper.h"
#include "DiscreteObject.h"
#include "DiscreteObjectParams.h"

#include <boost/numeric/ublas/matrix.hpp>

//#include "Jano/wrmnk.h"
#include "Jano/PreIdentifiedObject.h"
#include "Jano/Relay.h"

namespace RafMat
{
namespace SimEngine
{

/** Klasa regulatora GPC */
class GPCController : public ControllerBase
{
public:
    typedef boost::numeric::ublas::matrix<double> Matrix;
    typedef boost::numeric::ublas::identity_matrix<double> IdentityMatrix;

    typedef std::deque<double> SampleMemory;

public:
    explicit GPCController(SimInterface::ISimulator &isim,
                  unsigned H = 7, unsigned L = 4, double alpha = 0.8,
                  double rho = 0.7, unsigned dA = 2, unsigned dB = 2, unsigned dly = 0,
                  double initGain = 0.2);

    //
    // Implementacja IConfigurable
    //

    /** Ustawia parametr (\see IConfigurable::setParameter). */
    bool setParameter(const std::string& param, const std::string& value) throw(std::logic_error);

    /** Odczytuje parametr (\see IConfigurable::getParameter). */
    std::string getParameter(const std::string& param) const throw(std::logic_error);

    /**
        Resetuje regulator.
    */
    void reset();

    /** Resetuje regulator z uwzględnieniem (lub nie) czyszczenia pamięci. */
    void reset(bool clearMem);

protected:
    /**
        Wykonuje krok przeliczeń stanu regulatora GPC.
    */
    double simulateControllerStep(double setPoint, double processVar);

    /**
        Wyrzuca parametry do strumienia konfiguracyjnego.
    */
    SimFile::ConfigWriter& dumpSpecificPar(SimFile::ConfigWriter& o) const;

private:
    // Stałe konfiguracyjne
    static const std::string CONF_ALL;
    static const std::string CONF_GAIN;
    static const std::string CONF_H;
    static const std::string CONF_L;
    static const std::string CONF_RHO;
    static const std::string CONF_ALPHA;
    static const std::string CONF_DA;
    static const std::string CONF_DB;
    static const std::string CONF_DLY;

    /**
        Ustawia wszystkie parametry regulatora PID.
        Funkcja daje silną gwarancję bezpieczeństwa.

        \param [in] values Parametry regulatora w forme wektora [ ... ].
    */
    void safeSetParameters(const std::string& values);
public:
    /** Odwraca macierz (algorytm LU). */
    bool invertMatrix(const Matrix& input, Matrix& inv);
private:
    /** Oblicza odpowiedź skokową zidentyfikowanego modelu.
        Odpowiedź uformowana jest od razu w macierz Q
        o wymiarze HxL */
    Matrix simulateStepResponse();

    /** Symuluje model odniesienia wg aktualnych w. początkowych.
        \param [in] u0 Aktualne wyjście regulatora - wejście obiektu.
        \param [in] y0 Warunek początkowy - ostatnie wyjście obiektu.
    */
    std::vector<double> simulateReferenceObj(double u0, double y0);


    /** Symuluje odpowiedź swobodną. */
    std::vector<double> simulateFreeResponse();

    void updateHorizonts();

    /** Uaktualnia rząd obiektu (dA, dB i opóźnienie). */
    void updateOrder();

    /** Wybór zidentyfikowanego modelu w zalezności od wartości wyjściowej obiektu */
    //void chooseObject();

    void InitializeSignalSP();

    double m_gain;              ///< Wzmocnienie regulatora P (na czas ustalania identyf.).

    unsigned int m_H;           ///< Horyzont predykcji.
    unsigned int m_L;           ///< Horyzont sterowania.
    double m_alpha;             ///< Stała czasowa modelu odniesienia.
    double m_rho;               ///< Waga.

    // Parametry identyfikowanego modelu.
    unsigned int m_dA;
    unsigned int m_dB;
    unsigned int m_dly;

    unsigned int m_gpcDelay;    ///< Opóźnienie włączenia regulacji GPC.

    WrmnkARXWrapperSPtr m_rmnk;      ///< Klasa identyfikacji obiektów dynamicznych.

    //Jano::PreIdentifiedObjectSPtr m_preIdentifiedObject; //! Obiekt wstępnie zidentyfikowany
    Jano::PreIdentifiedObject m_preIdentifiedObject;
    Jano::RelaySPtr m_Relay; // Zręczny wskaźnik na przekaźnik z histerezą
    std::vector<double> m_predictionSP;
    unsigned int m_simulationTime;

    DiscreteObjectSPtr m_identifiedObject;  ///< Model rzeczywisty po identyfikacji.
    DiscreteObjectParamsSPtr m_identifiedParams; ///< Parametry powyższego modelu.

    //SampleMemory m_memU;        ///< Przeszłe próbki wyjścia regulatora.
    SampleMemory m_memY;        ///< Przeszłe próbki wejścia regulatora (od str. sprz.)
    //SampleMemory m_memY0;       ///< Przeszłe próbki odpowiedzi swobodnej.

    SampleMemory m_memDU;       ///< przyrosty delta U - przeszłe próbki.

    Matrix m_qT;                ///<

    // Macierze buforowe (pomocnicze).
    Matrix m_matTmp;
    Matrix m_matQT;

    ////// TEST /////////
    //Jano::WRMNK m_wrmnk;
};

}
}

#endif // GPCCONTROLLER_H
